function p = fitPlane(N)
M = [0 0 0 0 -1; 0 1 0 0 0 ; 0 0 1 0 0; 0 0 0 1 0 ; -1 0 0 0 0];
P = eye(5);
P(5,5) = 0;

p = zeros(5, N)

for i=1:N
    p(:, i) = [1 rand rand rand 0]';
    p(5, i) = 0.5 * (p(2, i) ^ 2 + p(3, i) ^ 2 + p(4, i) ^ 2);
end

ep = p(2:4, :);

sd = 0.5 * (trace(ep' * ep) - (sum(ep, 2)' * sum(ep, 2))) * eye(5)
s = p * p' * M

FD = P * (sd + s)
null(FD(1:end-1,:))



end